﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace AGVController
{
    public class Service
    {
        public TcpClient Client { get; set; }
        public Service(string agvIp, int agvPort)
        {
            Client = new TcpClient(agvIp, agvPort); 
        }

        public void SendToAgv(byte[] head, string data)
        {
            try
            {
                if (Client.Connected)
                {
                    NetworkStream serverStream = Client.GetStream();

                    var newmsg = new SeerMessage();

                    newmsg.head = bytesToStructure<SeerMessageHead>(head);
                    newmsg.data = normalStrToHexByte(data);

                    serverStream.Write(seerMessageHeadToBytes(newmsg.head), 0, Marshal.SizeOf(newmsg.head));
                    serverStream.Write(newmsg.data, 0, newmsg.data.Length);
                    serverStream.Flush();

                    byte[] inStream = new byte[16];
                    while (16 != serverStream.Read(inStream, 0, 16))
                    {
                        Thread.Sleep(20);
                    }

                    var recv_head = bytesToStructure<SeerMessageHead>(inStream);

                    byte[] recvbyte = BitConverter.GetBytes(recv_head.length);

                    Array.Reverse(recvbyte);

                    var dsize = BitConverter.ToUInt32(recvbyte, 0);

                    const int bufferSize = 512;
                    List<byte> datalist = new List<byte>();
                    int count = 0;

                    while (true)
                    {
                        byte[] buffer = new byte[bufferSize];
                        int readSize = serverStream.Read(buffer, 0, bufferSize);

                        count += readSize;
                        datalist.AddRange(buffer);

                        if (count == dsize)
                        {
                            break;
                        }

                        Thread.Sleep(10);
                    }

                    //textBox_recv_head.Text = BitConverter.ToString(seerMessageHeadToBytes(recv_head)).Replace("-", " ");//normalStrToHexStr(Encoding.UTF8.GetString(seerMessageHeadToBytes(recv_head)));

                    string str = Encoding.UTF8.GetString(datalist.ToArray());

                    //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = str; }));

                    Client.Close();
                }
            }
            catch (SocketException)
            {
                //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = ""; }));
                //MessageBox.Show("Connect Error!");
            }
            catch (IOException)
            {
                //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = ""; }));
                //MessageBox.Show("");
            }
        }

        public static T bytesToStructure<T>(byte[] bytesBuffer)
        {
            if (bytesBuffer.Length < Marshal.SizeOf(typeof(T)))
            {
                throw new ArgumentException("size error");
            }

            IntPtr bufferHandler = Marshal.AllocHGlobal(bytesBuffer.Length);
            for (int index = 0; index < bytesBuffer.Length; index++)
            {
                Marshal.WriteByte(bufferHandler, index, bytesBuffer[index]);
            }
            T structObject = (T)Marshal.PtrToStructure(bufferHandler, typeof(T));
            Marshal.FreeHGlobal(bufferHandler);
            return structObject;
        }

        public byte[] seerMessageHeadToBytes(SeerMessageHead msg)
        {
            var hsize = Marshal.SizeOf(msg);
            byte[] bytes = new byte[hsize];
            IntPtr structPtr = Marshal.AllocHGlobal(hsize);
            Marshal.StructureToPtr(msg, structPtr, false);
            Marshal.Copy(structPtr, bytes, 0, hsize);
            Marshal.FreeHGlobal(structPtr);
            return bytes;
        }

        private byte[] hexStrTobyte(string hexString)
        {
            hexString = hexString.Replace(" ", "");
            if ((hexString.Length % 2) != 0)
                hexString += " ";
            byte[] returnBytes = new byte[hexString.Length / 2];
            for (int i = 0; i < returnBytes.Length; i++)
                returnBytes[i] = Convert.ToByte(hexString.Substring(i * 2, 2).Trim(), 16);
            return returnBytes;
        }

        private byte[] normalStrToHexByte(string str)
        {
            byte[] result = new byte[str.Length];

            byte[] buffer = System.Text.Encoding.UTF8.GetBytes(str);
            for (int i = 0; i < buffer.Length; i++)
            {
                result[i] = Convert.ToByte(buffer[i].ToString("X2"), 16);
            }
            return result;
        }



        #region ADD BY CJH

        #region AGV路径导航流程

        //1. 机器人开机
        //2. 等待机器人启动完成（使用 TCP 连接机器人成功后即为启动完成）
        //3. 执行重定位 2002
        //4. 查询定位状态，等待定位完成1021
        //5. 确认定位正确 2003
        //6. 执行路径导航 3051
        //7. 查询导航状态，等待导航完成 1020
        //8. 循环执行步骤 6、7 即可实现持续的路径导航

        #endregion

        #region 定义调用变量
        private ushort number = 0;
        #endregion

        #region 给机器人发送指令 

        public string SendToAgv(SeerMessageHead head, byte[] data)
        {
            string strResult = "";
            try
            {
                if (Client.Connected)
                {
                    NetworkStream serverStream = Client.GetStream();

                    var newmsg = new SeerMessage();

                    newmsg.head = head;
                    newmsg.data = data;

                    serverStream.Write(seerMessageHeadToBytes(newmsg.head), 0, Marshal.SizeOf(newmsg.head));
                    serverStream.Write(newmsg.data, 0, newmsg.data.Length);
                    serverStream.Flush();

                    byte[] inStream = new byte[16];
                    while (16 != serverStream.Read(inStream, 0, 16))
                    {
                        Thread.Sleep(20);
                    }

                    var recv_head = bytesToStructure<SeerMessageHead>(inStream);

                    byte[] recvbyte = BitConverter.GetBytes(recv_head.length);

                    Array.Reverse(recvbyte);

                    var dsize = BitConverter.ToUInt32(recvbyte, 0);

                    const int bufferSize = 512;
                    List<byte> datalist = new List<byte>();
                    int count = 0;

                    while (true)
                    {
                        byte[] buffer = new byte[bufferSize];
                        int readSize = serverStream.Read(buffer, 0, bufferSize);

                        count += readSize;
                        datalist.AddRange(buffer);

                        if (count == dsize)
                        {
                            break;
                        }

                        Thread.Sleep(10);
                    }

                    //textBox_recv_head.Text = BitConverter.ToString(seerMessageHeadToBytes(recv_head)).Replace("-", " ");//normalStrToHexStr(Encoding.UTF8.GetString(seerMessageHeadToBytes(recv_head)));

                    string str = Encoding.UTF8.GetString(datalist.ToArray());

                    //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = str; }));

                    Client.Close();

                    strResult=str;
                }
            }
            catch (SocketException)
            {
                //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = ""; }));
                //MessageBox.Show("Connect Error!");
                strResult= "ConnectError";
            }
            catch (IOException)
            {
                //textBox_recv_data.Invoke(new EventHandler(delegate { textBox_recv_data.Text = ""; }));
                //MessageBox.Show("");

                strResult = "IOException";
            }
            return strResult;
        }

        private SeerMessageHead getSeerMessageHead(ushort type,uint length)
        {
            SeerMessageHead smh = new SeerMessageHead();
            smh.sync = 0x5A;
            smh.version = 0x01;
            smh.number = number > 65533 ? (ushort)0 : number++;
            smh.length = length;
            smh.type = type;
            return smh;
        }

        #endregion

        #region 机器人控制

        /// <summary>
        /// 机器人重定位
        /// </summary>
        public void robot_control_reloc_req(string data)
        {
            byte[] byteData = normalStrToHexByte(data);
            SendToAgv(getSeerMessageHead(2002,(uint)byteData.Length), byteData);
        }

        /// <summary>
        /// 确认定位正确
        /// </summary>
        public string robot_control_comfirmloc_req(string data)
        {
            byte[] byteData = normalStrToHexByte(data);
            return SendToAgv(getSeerMessageHead(2003, (uint)byteData.Length), byteData);
        }
        #endregion

        #region 机器人状态

        /// <summary>
        /// 查询机器人位置
        /// </summary>
        public string robot_status_loc_req(string data)
        {
            byte[] byteData = normalStrToHexByte(data);
            return SendToAgv(getSeerMessageHead(1004, (uint)byteData.Length), byteData);
        }
        /// <summary>
        /// 查询机器人定位状态
        /// </summary>
        public string robot_status_reloc_req(string data)
        {
            byte[] byteData = normalStrToHexByte(data);
            return SendToAgv(getSeerMessageHead(1021, (uint)byteData.Length), byteData);
        }

        /// <summary>
        /// 查询机器人导航状态
        /// </summary>
        public string robot_status_task_req(string data)
        {
            byte[] byteData = normalStrToHexByte(data);
            return SendToAgv(getSeerMessageHead(1020, (uint)byteData.Length), byteData);
        }

        /// <summary>
        /// 查询机器人电池状态
        /// </summary>
        public string robot_status_battery_req(string data)
        {
            byte[] byteData = normalStrToHexByte(data);
            return SendToAgv(getSeerMessageHead(1007, (uint)byteData.Length), byteData);
        }
        #endregion


        #region 机器人导航

        /// <summary>
        /// 路径导航
        /// </summary>
        public string robot_task_gotarget_req(string data)
        {
            byte[] byteData = normalStrToHexByte(data);
            return SendToAgv(getSeerMessageHead(3051, (uint)byteData.Length), byteData);
        }
        #endregion
        #endregion


    }

    public struct SeerMessageHead
    {
        public byte sync;
        public byte version;
        public UInt16 number;
        public UInt32 length;
        public UInt16 type;
        private byte ref0;      //保留
        private byte ref1;      //保留
        private byte ref2;      //保留
        private byte ref3;      //保留
        private byte ref4;      //保留
        private byte ref5;      //保留

    };
    public struct SeerMessage
    {
        public SeerMessageHead head;
        public byte[] data;
        public int length()
        {
            return data.Length + Marshal.SizeOf(head);
        }
    }
}
